import lcm
import sys
import os
import time
from threading import Thread, Lock
from motor_ctrl_lcmt import motor_ctrl_lcmt
from leg_control_data_lcmt import leg_control_data_lcmt

KD = 5.0
KP = 180.0

class Motor_Robot_Controller(object):
    rec_msg: leg_control_data_lcmt

    def __init__(self):
        self.rec_thread = Thread(target=self.rec_response)
        self.send_thread = Thread(target=self.send_publish)
        self.lc_r = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")
        self.lc_s = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")
        self.cmd_msg = motor_ctrl_lcmt()
        self.rec_msg = leg_control_data_lcmt()
        self.send_lock = Lock()
        self.delay_cnt = 0
        self.mode_ok = 0
        self.gait_ok = 0
        self.runing = 1
        self.currentQ = []

    def run(self):
        self.lc_r.subscribe("leg_control_data", self.msg_handler)
        self.send_thread.start()
        self.rec_thread.start()

    def msg_handler(self, channel, data):
        # print("handling message.")
        self.rec_msg = leg_control_data_lcmt().decode(data)        #type: ignore
        self.currentQ = self.rec_msg.q
        # print(f"\r{self.rec_msg.q}")
        # if(self.rec_msg.order_process_bar >= 95):
        #     self.mode_ok = self.rec_msg.mode
        # else:
        #     self.mode_ok = 0

    def rec_response(self):
        while self.runing:
            self.lc_r.handle()
            time.sleep( 0.002 )

    def Wait_finish(self, mode, gait_id):
        count = 0
        while self.runing and count < 2000: #10s
            if self.mode_ok == mode and self.gait_ok == gait_id:
                return True
            else:
                time.sleep(0.005)
                count += 1

    def send_publish(self):
        while self.runing:
            self.send_lock.acquire()
            if self.delay_cnt > 20: # Heartbeat signal 10HZ, It is used to maintain the heartbeat when life count is not updated
                self.lc_s.publish("motor_ctrl",self.cmd_msg.encode())
                self.delay_cnt = 0
            self.delay_cnt += 1
            self.send_lock.release()
            time.sleep( 0.005 )

    def Send_cmd(self, msg):
        self.send_lock.acquire()
        self.delay_cnt = 50
        self.cmd_msg = msg
        self.send_lock.release()

    def quit(self):
        self.runing = 0
        self.rec_thread.join()
        self.send_thread.join()





msg = motor_ctrl_lcmt()
Ctrl = Motor_Robot_Controller()
Ctrl.run()


def sendTestFrames(q_des=None, tau_des=None, kp_des=None, kd_des=None):
    if q_des is None:
        q_des = [0.0] * 12
    if tau_des is None:
        tau_des = [0.0] * 12
    if kp_des is None:
        kp_des = [KP] * 12
    if kd_des is None:
        kd_des = [KD] * 12

    msg.q_des = q_des
    msg.tau_des = tau_des
    msg.kp_des = kp_des
    msg.kd_des = kd_des
    Ctrl.Send_cmd(msg)
    print("sending test frame.")


def loopSendTestFrames():
    for _ in range(10000):
        sendTestFrames()
    


def sendZeroFrames():
    for _ in range(8):
        msg.q_des = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
        msg.qd_des = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
        msg.kp_des = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0] 
        msg.kd_des = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0] 
        msg.tau_des = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]
        Ctrl.Send_cmd(msg)
        time.sleep(0.5)
        print("sending zero frame.")


def test2():
    q_des = list(Ctrl.rec_msg.q)
    q_des[2] = 1.5
    q_des[5] = 1.5
    q_des[8] = 1.5
    q_des[11] = 1.5
    for _ in range(1000):
        sendTestFrames(q_des=q_des)
        time.sleep(0.005)



if __name__ == '__main__':
    sendZeroFrames()
    test2()
    Ctrl.quit()
    sys.exit()




